/*
 * File: SOC_AH.c
 *
 * Code generated for Simulink model 'SOC_AH'.
 *
 * Model version                  : 1.18
 * Simulink Coder version         : 8.11 (R2016b) 25-Aug-2016
 * C/C++ source code generated on : Thu Sep 10 14:31:28 2020
 *
 * Target selection: ert.tlc
 * Embedded hardware selection: STMicroelectronics->ST10/Super10
 * Code generation objectives: Unspecified
 * Validation result: Not run
 */

#include "SOC_AH.h"
#include "SOC_AH_private.h"

/* Imported (extern) block signals */
real_T BatSoc;                  /* '<S1>/Switch' */
real_T BatCurrent;              /* '<Root>/BatCurrent' */
real_T TempAvg;                 /* '<Root>/TempAvg' */
real_T SohHistory;              /* '<Root>/SohHistory' */
real_T SocHistory;              /* '<Root>/SocHistory' */
real_T CellVltgAvg;             /* '<Root>/CellVltgAvg' */
/* Block states (auto storage) */
DW_SOC_AH_T SOC_AH_DW;

/* External outputs (root outports fed by signals with auto storage) */
ExtY_SOC_AH_T SOC_AH_Y;

/* Real-time model */
RT_MODEL_SOC_AH_T SOC_AH_M_;
RT_MODEL_SOC_AH_T *const SOC_AH_M = &SOC_AH_M_;
static void rate_scheduler(void);
real_T look1_binlxpw(real_T u0, const real_T bp0[], const real_T table[],
                     uint32_T maxIndex)
{
  real_T frac;
  uint32_T iRght;
  uint32_T iLeft;
  uint32_T bpIdx;

  /* Lookup 1-D
     Search method: 'binary'
     Use previous index: 'off'
     Interpolation method: 'Linear'
     Extrapolation method: 'Linear'
     Use last breakpoint for index at or above upper limit: 'off'
     Remove protection against out-of-range input in generated code: 'off'
   */
  /* Prelookup - Index and Fraction
     Index Search method: 'binary'
     Extrapolation method: 'Linear'
     Use previous index: 'off'
     Use last breakpoint for index at or above upper limit: 'off'
     Remove protection against out-of-range input in generated code: 'off'
   */
  if (u0 <= bp0[0UL]) {
    iLeft = 0UL;
    frac = (u0 - bp0[0UL]) / (bp0[1UL] - bp0[0UL]);
  } else if (u0 < bp0[maxIndex]) {
    /* Binary Search */
    bpIdx = maxIndex >> 1UL;
    iLeft = 0UL;
    iRght = maxIndex;
    while (iRght - iLeft > 1UL) {
      if (u0 < bp0[bpIdx]) {
        iRght = bpIdx;
      } else {
        iLeft = bpIdx;
      }

      bpIdx = (iRght + iLeft) >> 1UL;
    }

    frac = (u0 - bp0[iLeft]) / (bp0[iLeft + 1UL] - bp0[iLeft]);
  } else {
    iLeft = maxIndex - 1UL;
    frac = (u0 - bp0[maxIndex - 1UL]) / (bp0[maxIndex] - bp0[maxIndex - 1UL]);
  }

  /* Interpolation 1-D
     Interpolation method: 'Linear'
     Use last breakpoint for index at or above upper limit: 'off'
     Overflow mode: 'portable wrapping'
   */
  return (table[iLeft + 1UL] - table[iLeft]) * frac + table[iLeft];
}

real_T look2_binlxpw(real_T u0, real_T u1, const real_T bp0[], const real_T bp1[],
                     const real_T table[], const uint32_T maxIndex[], uint32_T
                     stride)
{
  real_T frac;
  uint32_T bpIndices[2];
  real_T fractions[2];
  real_T yL_1d;
  uint32_T iRght;
  uint32_T bpIdx;
  uint32_T iLeft;

  /* Lookup 2-D
     Search method: 'binary'
     Use previous index: 'off'
     Interpolation method: 'Linear'
     Extrapolation method: 'Linear'
     Use last breakpoint for index at or above upper limit: 'off'
     Remove protection against out-of-range input in generated code: 'off'
   */
  /* Prelookup - Index and Fraction
     Index Search method: 'binary'
     Extrapolation method: 'Linear'
     Use previous index: 'off'
     Use last breakpoint for index at or above upper limit: 'off'
     Remove protection against out-of-range input in generated code: 'off'
   */
  if (u0 <= bp0[0UL]) {
    iLeft = 0UL;
    frac = (u0 - bp0[0UL]) / (bp0[1UL] - bp0[0UL]);
  } else if (u0 < bp0[maxIndex[0UL]]) {
    /* Binary Search */
    bpIdx = maxIndex[0UL] >> 1UL;
    iLeft = 0UL;
    iRght = maxIndex[0UL];
    while (iRght - iLeft > 1UL) {
      if (u0 < bp0[bpIdx]) {
        iRght = bpIdx;
      } else {
        iLeft = bpIdx;
      }

      bpIdx = (iRght + iLeft) >> 1UL;
    }

    frac = (u0 - bp0[iLeft]) / (bp0[iLeft + 1UL] - bp0[iLeft]);
  } else {
    iLeft = maxIndex[0UL] - 1UL;
    frac = (u0 - bp0[maxIndex[0UL] - 1UL]) / (bp0[maxIndex[0UL]] - bp0[maxIndex
      [0UL] - 1UL]);
  }

  fractions[0UL] = frac;
  bpIndices[0UL] = iLeft;

  /* Prelookup - Index and Fraction
     Index Search method: 'binary'
     Extrapolation method: 'Linear'
     Use previous index: 'off'
     Use last breakpoint for index at or above upper limit: 'off'
     Remove protection against out-of-range input in generated code: 'off'
   */
  if (u1 <= bp1[0UL]) {
    iLeft = 0UL;
    frac = (u1 - bp1[0UL]) / (bp1[1UL] - bp1[0UL]);
  } else if (u1 < bp1[maxIndex[1UL]]) {
    /* Binary Search */
    bpIdx = maxIndex[1UL] >> 1UL;
    iLeft = 0UL;
    iRght = maxIndex[1UL];
    while (iRght - iLeft > 1UL) {
      if (u1 < bp1[bpIdx]) {
        iRght = bpIdx;
      } else {
        iLeft = bpIdx;
      }

      bpIdx = (iRght + iLeft) >> 1UL;
    }

    frac = (u1 - bp1[iLeft]) / (bp1[iLeft + 1UL] - bp1[iLeft]);
  } else {
    iLeft = maxIndex[1UL] - 1UL;
    frac = (u1 - bp1[maxIndex[1UL] - 1UL]) / (bp1[maxIndex[1UL]] - bp1[maxIndex
      [1UL] - 1UL]);
  }

  /* Interpolation 2-D
     Interpolation method: 'Linear'
     Use last breakpoint for index at or above upper limit: 'off'
     Overflow mode: 'portable wrapping'
   */
  bpIdx = iLeft * stride + bpIndices[0UL];
  yL_1d = (table[bpIdx + 1UL] - table[bpIdx]) * fractions[0UL] + table[bpIdx];
  bpIdx += stride;
  return (((table[bpIdx + 1UL] - table[bpIdx]) * fractions[0UL] + table[bpIdx])
          - yL_1d) * frac + yL_1d;
}

/*
 *   This function updates active task flag for each subrate.
 * The function is called at model base rate, hence the
 * generated code self-manages all its subrates.
 */
static void rate_scheduler(void)
{
  /* Compute which subrates run during the next base time step.  Subrates
   * are an integer multiple of the base rate counter.  Therefore, the subtask
   * counter is reset when it reaches its limit (zero means run).
   */
  (SOC_AH_M->Timing.TaskCounters.TID[1])++;
  if ((SOC_AH_M->Timing.TaskCounters.TID[1]) > 9) {/* Sample time: [0.01s, 0.0s] */
    SOC_AH_M->Timing.TaskCounters.TID[1] = 0;
  }
}

/* Model step function */
void SOC_AH_step(void)
{
  real_T rtb_Switch;
  real_T rtb_Product3;
  if (SOC_AH_M->Timing.TaskCounters.TID[1] == 0) {
    /* Gain: '<S2>/Gain' incorporates:
     *  Inport: '<Root>/CellVltgAvg'
     *  Lookup_n-D: '<S2>/SOC_Corr_OCV'
     */
    rtb_Switch = 0.01 * look1_binlxpw(CellVltgAvg,
      SOC_AH_ConstP.SOC_Corr_OCV_bp01Data, SOC_AH_ConstP.SOC_Corr_OCV_tableData,
      19UL);

    /* Switch: '<S2>/Switch' incorporates:
     *  Abs: '<S2>/Abs1'
     *  Constant: '<S2>/Constant'
     *  Inport: '<Root>/SocHistory'
     *  RelationalOperator: '<S2>/Relational Operator'
     *  Sum: '<S2>/Subtract'
     */
    if (fabs(SocHistory - rtb_Switch) <= 0.05) {
      rtb_Switch = SocHistory;
    }

    /* End of Switch: '<S2>/Switch' */

    /* Product: '<S1>/Product3' incorporates:
     *  Constant: '<S1>/AS'
     *  Constant: '<S1>/Constant8'
     *  Inport: '<Root>/SohHistory'
     */
    rtb_Product3 = 27000.0 * SohHistory;

    /* DiscreteIntegrator: '<S1>/Discrete-Time Integrator' incorporates:
     *  Product: '<S1>/Product4'
     */
    if (SOC_AH_DW.DiscreteTimeIntegrator_IC_LOADI != 0) {
      SOC_AH_DW.DiscreteTimeIntegrator_DSTATE = rtb_Switch * rtb_Product3;
    }

    /* Product: '<S1>/Divide4' incorporates:
     *  DiscreteIntegrator: '<S1>/Discrete-Time Integrator'
     */
    rtb_Product3 = SOC_AH_DW.DiscreteTimeIntegrator_DSTATE / rtb_Product3;

    /* Switch: '<S1>/Switch' incorporates:
     *  Abs: '<S1>/Abs'
     *  Constant: '<S1>/Constant'
     *  Constant: '<S1>/Constant1'
     *  Constant: '<S1>/Constant2'
     *  Constant: '<S1>/Constant6'
     *  Inport: '<Root>/BatCurrent'
     *  Inport: '<Root>/TempAvg'
     *  Lookup_n-D: '<S1>/BMS_Capacity'
     *  Lookup_n-D: '<S1>/BMS_DchaRate'
     *  Product: '<S1>/Divide'
     *  Product: '<S1>/Divide1'
     *  Product: '<S1>/Product2'
     *  RelationalOperator: '<S1>/Relational Operator'
     */
    if (BatCurrent <= 0.0) {
      BatSoc = 7.5 / look2_binlxpw(TempAvg, 100.0,
        SOC_AH_ConstP.BMS_Capacity_bp01Data, SOC_AH_ConstP.BMS_Capacity_bp02Data,
        SOC_AH_ConstP.BMS_Capacity_tableData,
        SOC_AH_ConstP.BMS_Capacity_maxIndex, 5UL) * (7.5 / look1_binlxpw(fabs
        (BatCurrent), SOC_AH_ConstP.BMS_DchaRate_bp01Data,
        SOC_AH_ConstP.BMS_DchaRate_tableData, 8UL)) * rtb_Product3;
    } else {
      BatSoc = rtb_Product3;
    }

    /* End of Switch: '<S1>/Switch' */

    /* Outport: '<Root>/SOC_InitCorr' */
    SOC_AH_Y.SOC_InitCorr = rtb_Switch;

    /* Update for DiscreteIntegrator: '<S1>/Discrete-Time Integrator' incorporates:
     *  Update for Inport: '<Root>/BatCurrent'
     */
    SOC_AH_DW.DiscreteTimeIntegrator_IC_LOADI = 0U;
    SOC_AH_DW.DiscreteTimeIntegrator_DSTATE += 0.01 * BatCurrent;
  }

  rate_scheduler();
}

/* Model initialize function */
void SOC_AH_initialize(void)
{
  /* Registration code */

  /* initialize real-time model */
  (void) memset((void *)SOC_AH_M, 0,
                sizeof(RT_MODEL_SOC_AH_T));

  /* states (dwork) */
  (void) memset((void *)&SOC_AH_DW, 0,
                sizeof(DW_SOC_AH_T));

  /* external outputs */
  (void) memset((void *)&SOC_AH_Y, 0,
                sizeof(ExtY_SOC_AH_T));

  /* InitializeConditions for DiscreteIntegrator: '<S1>/Discrete-Time Integrator' */
  SOC_AH_DW.DiscreteTimeIntegrator_IC_LOADI = 1U;
}

/* Model terminate function */
void SOC_AH_terminate(void)
{
  /* (no terminate code required) */
}

/*
 * File trailer for generated code.
 *
 * [EOF]
 */
